#!/usr/bin/env python3
"""
Start MoveIt 2 move_group for RobotOne arm with ros2_control controllers.
"""

import os
import yaml

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, FindExecutable
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory


def load_file(package_name: str, relative_path: str) -> str:
    file_path = os.path.join(get_package_share_directory(package_name), relative_path)
    with open(file_path, "r", encoding="utf-8") as file:
        return file.read()


def load_yaml(package_name: str, relative_path: str):
    return yaml.safe_load(load_file(package_name, relative_path))


def generate_launch_description():
    use_sim_time_arg = DeclareLaunchArgument(
        "use_sim_time",
        default_value="true",
        description="Use simulation time",
    )

    description_pkg = FindPackageShare("arm")
    xacro_file = PathJoinSubstitution([description_pkg, "urdf", "robotone_arm.urdf.xacro"])
    robot_description = {"robot_description": Command([FindExecutable(name="xacro"), " ", xacro_file])}

    robot_description_semantic = {
        "robot_description_semantic": load_file("arm_moveit_config", "srdf/robotone_arm.srdf")
    }

    robot_description_kinematics = {
        "robot_description_kinematics": load_yaml("arm_moveit_config", "config/kinematics.yaml")
    }
    ompl_config = {"robot_description_planning": load_yaml("arm_moveit_config", "config/ompl_planning.yaml")}
    joint_limits = {"robot_description_joint_limits": load_yaml("arm_moveit_config", "config/joint_limits.yaml")}
    planning_pipelines = load_yaml("arm_moveit_config", "config/planning_pipelines.yaml")
    moveit_controllers = {
        "moveit_simple_controller_manager": load_yaml(
            "arm_moveit_config", "config/moveit_controllers.yaml"
        )
    }
    planning_scene_monitor_params = load_yaml(
        "arm_moveit_config", "config/planning_scene_monitor_parameters.yaml"
    )

    parameters = [
        robot_description,
        robot_description_semantic,
        robot_description_kinematics,
        ompl_config,
        joint_limits,
        moveit_controllers,
        planning_pipelines,
        planning_scene_monitor_params,
        {"use_sim_time": LaunchConfiguration("use_sim_time")},
    ]

    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=parameters,
    )

    return LaunchDescription([use_sim_time_arg, move_group_node])

